#include <stdlib.h>
#include <math.h>
#include "CAStar.h"
#include "CAStar_private.h"

Nodes nodes;
NodeList openList, closedList; //list[areaWidth*areaHeight]
Neighbors neighbors;

int isInited = 0;
int isFreed = 0;

void CAStar_private_List_add(NodeList* nodeList, Node* n) {
	if(n!= NULL && nodeList->nodePositionHash[n->x*area.height + n->y] == NULL) {
		nodeList->size++;
	}
	int pos = n->x*area.height + n->y;
	nodeList->nodePositionHash[pos] = n;
}

void CAStar_private_List_remove(NodeList* nodeList, int x, int y) {
	if(nodeList->size==0 || x >= area.width || y >= area.height) {
		return;
	}
	Node* existingN = nodeList->nodePositionHash[x*area.height + y];
	if(existingN == NULL) {
		return;
	}
	nodeList->nodePositionHash[x*area.height + y] = NULL;
	nodeList->size--;
}

Node* CAStar_private_List_getFirst(NodeList* nodeList) {
	if(nodeList->size <= 0) {
		return NULL;
	}
	int x, y;
	for(x=0; x < area.width; x++) {
		for(y=0; y < area.height; y++) {
			Node* n = nodeList->nodePositionHash[x*area.height + y];
			if(n != NULL) {
				return n;
			}
		}
	}
	return NULL;
}

Node* CAStar_private_List_searchByPosition(NodeList* nodeList, int x, int y) {
	return nodeList->nodePositionHash[x*area.height + y];
}

Node* CAStar_private_List_getLowestFNode() {
	if(openList.size == 0) {
		return NULL;
	}

	Node* lowestFNode = NULL;
	int x, y;
	for(x=0; x < area.width; x++) {
		for(y=0; y < area.height; y++) {
			Node* n = CAStar_private_List_searchByPosition(&openList, x, y);
			if(n!=NULL) {
				if(lowestFNode==NULL) {
					lowestFNode=n;
				} else {
					double nF = CAStar_private_getF(n);
					double lowestFNodeF = CAStar_private_getF(lowestFNode);
					if(nF < lowestFNodeF) {
						lowestFNode = n;
					}
				}
			}
		}

	}

	return lowestFNode;
}

void CAStar_private_processSucessors(Node* currentNode, Node* finish, int somewhatPassableGAddendum) {
	CAStar_private_calculateNeighbors(currentNode);
	int i;
	for(i = 0; i<neighbors.size; i++) {
		Node* neighbor = neighbors.neighbors[i];

		//if neighbor node is already IN THE CLOSED LIST, skip it:
		Node* closedNode = CAStar_private_List_searchByPosition(&closedList, neighbor->x, neighbor->y);
		if(closedNode != NULL) {
			continue;
		}

		//calculate the cost of getting to the neighbor THROUGH the currently analyzed node:
		double newG = currentNode->g + CAStar_private_getDistance(currentNode, neighbor);

		//if this neighbor node contains somewhat passable terrain, add some extra value to it's g cost:
		//TODO: this should be made up so that each node can have a specific value for it's extra g cost
		//(and not simply a constant which applies to all somewhat passable terrain nodes):
		if(CAStar_getTerrainAt(neighbor->x, neighbor->y)==TERRAIN_SOME_WHAT_PASSABLE) {
			newG = newG+somewhatPassableGAddendum;
		}

		//if the neighbor is already IN THE OPEN LIST *AND* IT'S THERE WITH A LOWER G COST, skip it
		//as this means that this neighbor is already present in some other path (has some other parent) which is better than
		//what we're investigating right now:
		Node* openNode = CAStar_private_List_searchByPosition(&openList, neighbor->x, neighbor->y);
		if (openNode!=NULL && openNode->g <= newG) {
			continue;
		}

		//if we're here, that means that this neighbor represents a node worth investigating as a potential member of the best path

		//if this neighbor is present in the open list, but with a worse (higher) g score, then remove it from the opened list
		//this means that this neighbor has made it to the open list already, but with a parent which constitutes for a path which is
		//longer (costlier) than if it were to go through the currently analyzed node (have it as parent)
		if(openNode!=NULL) {
			CAStar_private_List_remove(&openList, neighbor->x, neighbor->y);
		}

		/*
		 * at this point we know that this neighbor is:
		 * - not on the closed list
		 * - is walkable (does not contain impassable terrain)
		 * - either
		 *    - not on the open list
		 *    - on the open list, but with a g cost higher than if it were to pass through the currently analyzed node
		 *    (aka: if we replace it's current parent with the currently analyzed node, it will make for a less costly (shorter) path
		 *
		 * Set it's g and h scores, set the currently analyzed node as its parent, and add it to the opened list:    		 *
		 */
		neighbor->g = newG;
		neighbor->h = CAStar_private_getDistance(neighbor,finish);
		CAStar_private_addParent(neighbor, currentNode);
		if(lastCalculatedPath.closestNode == NULL || lastCalculatedPath.closestNode->h > neighbor->h) {
			lastCalculatedPath.closestNode = neighbor;
		}
		CAStar_private_List_add(&openList, neighbor);
	}
}

void CAStar_private_addParent(Node* child, Node* parent) {
	child->parent = parent;
	child->parentsCount = parent->parentsCount + 1;
}

int CAStar_private_Node_equals(Node* n1, Node* n2) {
	if(n1->x == n2->x && n1->y == n2->y) {
		return 1;
	} else {
		return 0;
	}
}

double CAStar_private_getDistance(Node* n1, Node* n2) {
	double val = sqrt(
				(pow(UNIT_SIZE*(n1->x - n2->x), 2) +
				pow(UNIT_SIZE*(n1->y - n2->y), 2))
				 );
	return val;
}

void CAStar_private_calculateNeighbors(Node* node) {
	Node* leftN = NULL;
	Node* topN = NULL;
	Node* rightN = NULL;
	Node* bottomN = NULL;
	Node* leftUpperN = NULL;
	Node* rightUpperN = NULL;
	Node* rightLowerN = NULL;
	Node* leftLowerN = NULL;

	int nCount = 0;
	if(node->x > 0 && CAStar_getTerrainAt(node->x-1, node->y) != TERRAIN_IMPASSABLE) {
		nCount++;
		leftN = nodes.nodes[(node->x-1) * area.height + node->y];
	}
	if(node->x < area.width-1 && CAStar_getTerrainAt(node->x+1, node->y) != TERRAIN_IMPASSABLE) {
		nCount++;
		rightN = nodes.nodes[(node->x+1) * area.height + node->y];
	}
	if(node->y > 0 && CAStar_getTerrainAt(node->x, node->y-1)!=TERRAIN_IMPASSABLE) {
		nCount++;
		topN = nodes.nodes[(node->x) * area.height + node->y-1];
	}
	if(node->y < area.height-1 && CAStar_getTerrainAt(node->x, node->y+1) != TERRAIN_IMPASSABLE) {
		nCount++;
		bottomN = nodes.nodes[(node->x) * area.height + node->y+1];
	}

	if(node->x > 0 && node->y > 0 && CAStar_getTerrainAt(node->x-1, node->y-1) != TERRAIN_IMPASSABLE) {
		nCount++;
		leftUpperN = nodes.nodes[(node->x-1) * area.height + node->y-1];
	}
	if(node->x > 0 && node->y < area.height-1 && CAStar_getTerrainAt(node->x-1, node->y+1) != TERRAIN_IMPASSABLE) {
		nCount++;
		leftLowerN  = nodes.nodes[(node->x-1) * area.height + node->y+1];
	}
	if(node->x < area.width-1 && node->y > 0 && CAStar_getTerrainAt(node->x+1, node->y-1) != TERRAIN_IMPASSABLE) {
		nCount++;
		rightUpperN = nodes.nodes[(node->x+1) * area.height + node->y-1];
	}
	if(node->x < area.width-1 && node->y < area.height-1 && CAStar_getTerrainAt(node->x+1, node->y+1) != TERRAIN_IMPASSABLE) {
		nCount++;
		rightLowerN  = nodes.nodes[(node->x+1) * area.height + node->y+1];
	}

	int posCount = 0;

	if(leftN!=NULL) {
		neighbors.neighbors[posCount] = leftN;
		posCount++;
	}

	if(topN!=NULL) {
		neighbors.neighbors[posCount] = topN;
		posCount++;
	}

	if(rightN!=NULL) {
		neighbors.neighbors[posCount] = rightN;
		posCount++;
	}

	if(bottomN!=NULL) {
		neighbors.neighbors[posCount] = bottomN;
		posCount++;
	}

	if(leftUpperN!=NULL) {
		neighbors.neighbors[posCount] = leftUpperN;
		posCount++;
	}

	if(leftLowerN!=NULL) {
		neighbors.neighbors[posCount] = leftLowerN;
		posCount++;
	}

	if(rightUpperN!=NULL) {
		neighbors.neighbors[posCount] = rightUpperN;
		posCount++;
	}

	if(rightLowerN!=NULL) {
		neighbors.neighbors[posCount] = rightLowerN;
		posCount++;
	}

	neighbors.size=posCount;
}

double CAStar_private_getF(Node* node) {
	if(node==NULL || node->g==-123 || node->h==-123) {
		return -1;
	}
	return node->g + node->h;
}

char* CAStar_private_Area_toString() {
	int strLength = (area.height * (area.width+1) + 1);
	char* str = malloc(strLength * sizeof(char));

	int x = 0, y = 0, pos=0;
	for(y=0; y < area.height; y++) {
		for(x=0; x < area.width; x++) {
			if(area.terrain[x*area.height+y] == TERRAIN_PASSABLE) {
				str[pos] = TERRAIN_PASSABLE_CHAR;
			} else if(area.terrain[x*area.height+y] == TERRAIN_IMPASSABLE) {
				str[pos] = TERRAIN_IMPASSABLE_CHAR;
			} else if(area.terrain[x*area.height+y] == TERRAIN_SOME_WHAT_PASSABLE) {
				str[pos] = TERRAIN_SOME_WHAT_PASSABLE_CHAR;
			}
			pos = pos+1;
		}
		str[pos]='\n';
		pos=pos + 1;
	}
	str[pos]='\0';

	return str;
}
